/******************************************************************************

				版权所有 (C), 2023, 威海众鼎智能科技有限公司

 ******************************************************************************
  文 件 名	: User.c
  版 本 号	: V2.01
  硬件信息	: ZHD_039
  作	者	: ZhangFY
  生成日期	: 2023.10.20
  功能描述	: 山沃静液压叉装车主控制器
  修改历史
  1.日    期   : 2023.10.20
	作    者   : ZhangFY
	修改内容   : 适配Danfoss泵马达曲线，踩下刹车后为泵增加可调斜坡
  2.日    期   : 2023.10.24
	作    者   : ZhangFY
	修改内容   : 增加寸进电流调试
******************************************************************************/

#include "User.h"
#define CoilResistance 14		   // Danfoss比例阀线圈电阻
u16 count_id, init_bb, time_delay; // 消息ID
u8 m_wait_gps = 0;				   // 请求数据后等网络恢复数据
u8 m1, sys_tx_gps[100], sys_rx_gps[100], len_sys_gps;
u8 sys_rx_ic[50], len_sys_ic;
u8 sys_rx_print[50], len_sys_print;
u8 delay_opt;
u8 LW_Enable;
u8 motor_x[6], motor_y[6], JF_SUB[6];
u32 JF_user;
u8 sys_tx_qdb[30], sys_rx_qdb[30], len_sys_qdb, arry_m[50], sys_rx_ic_m[30];
u16 time_cyc = 1500; // 未接收到请求循环再次发送数据间隔 10ms
__int64 ID;

//------------------------------------------全局变量 GLOBAL_VAR--------------------------------
u16 can1_count, can2_count;
u16 PI_Filter(u16 PI_IN);
u8 Tx_101[8], Tx_102[8], Tx_103[8], Tx_104[8], Tx_105[8], Tx_106[8], Tx_107[8], Tx_108[8], Tx_109[8], Tx_110[8], Tx_111[8], Tx_10A[8], Tx_10B[8], Tx_10C[8], Tx_121[8];
u8 Rx_201[8], Rx_202[8], Rx_203[8], Rx_204[8], Rx_205[8], Rx_206[8], Rx_207[8], Rx_208[8];
u8 Rx_28A[8]; // 长角传感器报文
u8 Tx_SL[8];
u8 SiLun_MoShi_SB;																				  // 四轮模式开关
u8 XieXing_MoShi_SB;																			  // 蟹行模式开关
u8 QianQiao_SB_1;																				  // 前桥接近开关
u8 HouQiao_SB_1;																				  // 后桥接近开关
u8 YQ_Xie, YQ_Si;																				  // 四轮转向，蟹行越权
u16 Eng_SP_Freq;																				  // 发动机转速传感器频率
u16 Eng_SP_Freq_1;																				  // 发动机转速传感器频率
u16 Motor_SP_Freq;																				  // 马达转速传感器频率
u16 Motor_Speed;																				  // 马达转速
u16 XingChe_SuDu;																				  //
/**/																							  //
u8 DW_QianJin;																					  // 前进挡开关
u8 DW_HouTui;																					  // 后退挡开关
u8 SiLun_Fa;																					  // 四轮转向阀
u8 XieXing_Fa;																					  // 蟹行转向阀
u8 LP_LiangLun;																					  // 两轮转向模式指示灯
u8 LP_LiangLun_S;																				  // 两轮转向模式指示灯
u8 LP_SiLun;																					  // 四轮转向模式指示灯
u8 LP_SiLun_S;																					  // 四轮转向模式指示灯
u8 LP_XieXing;																					  // 蟹行转向模式指示灯
u8 LP_XieXing_S;																				  // 蟹行转向模式指示灯
u16 XingZou_QJ_Fa;																				  // 行走泵前进阀
u16 XingZou_QJ_Fa1;																				  // 行走泵前进阀
u16 XingZou_HT_Fa;																				  // 行走泵后退阀
u16 XingZou_HT_Fa1;																				  // 行走泵后退阀
u16 XingZou_MD_Fa;																				  // 行走马达阀
u8 ShouDong_YQ = 0;																				  // 转向系统手动越权
u8 DiSu_MoShi = 1;																				  // 马达大排量模式
u8 WuGui_MoShi;																					  // 变速箱乌龟档模式
u8 ShaChe_XH;																					  // 刹车信号
u8 HuanDang_XH = 0;																					  // 换挡信号
u8 PingHengFa_QD;																				  // 平衡阀切断阀
u8 MD_SP_SUB = 0;																				  //
u8 MD_SP_ADD = 0;																				  //
u8 MD_SP_SUB1 = 0;																				  //
u8 D1, D2, N, R1, R2;																			  //
u16 Eng_Speed_SJ;																				  //
u16 System_Voltage;																				  // 系统电压
u16 Beng_IncTm_TZ, Beng_DecTm_WG, Motor_IncTm_TZ, Motor_IncTm_WG, Motor_DecTm_TZ, Motor_DecTm_WG; //
u16 Beng_IncTm, Beng_DecTm, Motor_IncTm, Motor_DecTm;											  //
u16 CunJin_Current;																				  // 寸进电流值
/**/																							  //
u8 RS232_TX_10[11], RS232_TX_11[11], RS232_TX_12[11], RS232_TX_13[11], RS232_TX_14[11], RS232_TX_15[11], RS232_TX_16[9], RS232_TX_17[9], RS232_TX_18[9], RS232_TX_19[9],
	RS232_TX_20[9], RS232_TX_21[9], RS232_TX_22[9], RS232_TX_23[9], RS232_TX_24[41];
u8 RS232_buff[9], RS232_RX[9];
u8 Ser_Head = 0x5A;
u8 Ser_Frame_0 = 0xA5;
u8 Ser_Frame_1 = 0x5A;
u8 Ser_Frame_2 = 0xA5;
u8 Ser_Frame_3 = 0xA5;
u16 YouGanQiang_YaLi_Bar;											  // 有杆腔压力  0.1 Bar
u16 WuGanQiang_YaLi_Bar;											  // 无杆腔压力  0.1 Bar
u16 i1, i2, i3;														  // 循环变量
/**/																  //
u8 Rx_281[8], Rx_282[8], Rx_283[8], Rx_284[8];						  //
u8 Rx_CJ181[8], Rx_CJ281[8];										  //
u8 Tx_181[8], Tx_182[8], Tx_183[8], Tx_184[8];						  //
/** 输入 **/														  //
u16 YouGanQiang_YaLi_mA;											  // 有杆腔压力传感器 mA 值
u16 WuGanQiang_YaLi_mA;												  // 无杆腔压力传感器 mA 值
u16 DangWei_QianHou_mV;												  // 前后挡拨轮
u8 DangWei_N;														  // N 档按钮
u8 DangWei_M;														  // M 档按钮
u8 CunJin_BUT;														  // 寸进按钮
u16 ShouBing_Y_mV;													  // 手柄 Y 轴 mV 值
u16 ShouBing_X_mV;													  // 手柄 Y 轴 mV 值
u16 ShenSuo_BL_mV;													  // 伸缩拨轮 mV 值
u16 FuJu_BL_mV;														  // 辅具拨轮 mV 值
u16 Eng_Speed_JS, Eng_Speed, Eng_Speed1, Eng_Speed_MD, Eng_Speed_MD1; // 发动机转速
/** 输出 **/														  //
u16 FuJu_Valve_a;													  // 辅具阀a
u16 FuJu_Valve_b;													  // 辅具阀b
u16 Boom_UP;														  // 大臂升
u16 Boom_UP1;														  // 大臂升
u16 Boom_DOWN;														  // 大臂降
u16 Boom_DOWN1;														  // 大臂降
u16 FanDou_UP;														  // 翻斗
u16 FanDou_DOWN;													  // 收斗
u16 Boom_Shen;														  // 大臂伸出
u16 Boom_Shen1;														  // 大臂伸出
u16 Boom_Suo;														  // 大臂缩回
u16 Boom_Suo1;														  // 大臂缩回
/**函数**/
u16 RC_Speed(u16 in, u8 len, u8 cyc, u16 *p, u32 *p2);
u16 Dec_Option(u16 in, u16 par_dec, u16 par_delay, u16 *p);
u32 Dec_Option_32(u32 in, u16 par_dec, u16 par_delay, u32 *p);
u32 Inc_Option(u32 in, u16 par_dec, u16 par_delay, u32 *p);
u16 OUT, ARR[100];
u32 ARR2[5];
u16 OUT2, ARR4[5];
u32 ARR3[5], ARR5[5], ARR6[5], ARR7[5], ARR8[5];
//------------------------------------------END_VAR--------------------------------
//===============================上升沿===========================================
u8 R_Trig(u8 in, u8 *p)
{
	if (in == 1 && (*p) == 0)
	{
		*p = in;
		return 1;
	}
	else
	{
		*p = in;
		return 0;
	}
}
//================================END_上升沿==========================================
//===============================下降沿===========================================
u8 F_Trig(u8 in, u8 *p)
{
	if (in == 0 && (*p) == 1)
	{
		*p = in;
		return 1;
	}
	else
	{
		*p = in;
		return 0;
	}
}
//================================END_下降沿==========================================
//------------------------------------------FLASH(EEPROM)-------------------------------------
#define FLASH_SAVE_ADDR 0X08040000 // 设置FLASH 保存地址(必须为偶数，且所在扇区,要大于本代码所占用到的扇区.
								   // 否则,写操作的时候,可能会导致擦除整个扇区,从而引起部分程序丢失.引起死机.
u8 flash_byte[500];
u8 eeprom_En[1005], eeprom_En_TD[3];
u16 Nub_eeprom, total_eeprom, Nub_Print, mark_read, count_id2 = 111, CYC_G;
//
u8 Key_Ctrl_L, Key_Ctrl_R, Key_Ctrl_Enable, Pot_Ctrl, Mode_Ctrl;
//
u8 p_u8[15], CAN_ERR;
int p_int[15];
//
// u16 Joy_Fliter(u16 IN,u16 Par_min,u16 Par_max);

// #########################1.系统初始化##############################
int KOP_Init(void)
{
	// 1.串口设置
	// ##########################大彩串口屏通讯###########################
	uart1_init(Sys_Config.usart_Congig.BaudRate.BaudRate_115200,
			   Sys_Config.usart_Congig.USART_WordLength.WordLength_8b,
			   Sys_Config.usart_Congig.USART_StopBits.StopBits_1,
			   Sys_Config.usart_Congig.Parity.Parity_No);
	// 3.CAN0口配置
	CAN1_Mode_Init(250);
	// 4.CAN1口配置
	CAN2_Mode_Init(250);
	return (200);
}
void CAN1_RX_Init(void)
{
	can_receive_setting1(0x201, CAN_ID_STD, 1, CAN_Filter_FIFO0);
	can_receive_setting1(0x202, CAN_ID_STD, 2, CAN_Filter_FIFO0);
	can_receive_setting1(0x203, CAN_ID_STD, 3, CAN_Filter_FIFO0);
	can_receive_setting1(0x204, CAN_ID_STD, 4, CAN_Filter_FIFO0);
	can_receive_setting1(0x205, CAN_ID_STD, 5, CAN_Filter_FIFO0);
	can_receive_setting1(0x206, CAN_ID_STD, 6, CAN_Filter_FIFO0);
	can_receive_setting1(0x207, CAN_ID_STD, 7, CAN_Filter_FIFO0);
	can_receive_setting1(0x208, CAN_ID_STD, 8, CAN_Filter_FIFO0);
	can_receive_setting1(0x209, CAN_ID_STD, 9, CAN_Filter_FIFO0);
	can_receive_setting1(0x20A, CAN_ID_STD, 10, CAN_Filter_FIFO0);
}
void CAN2_RX_Init(void)
{
	can_receive_setting2(0x28A, CAN_ID_STD, 14, CAN_Filter_FIFO0);
	can_receive_setting2(0x291, CAN_ID_STD, 15, CAN_Filter_FIFO0);
	can_receive_setting2(0x292, CAN_ID_STD, 16, CAN_Filter_FIFO0);
	can_receive_setting2(0x181, CAN_ID_STD, 17, CAN_Filter_FIFO0);
	can_receive_setting2(0x281, CAN_ID_STD, 18, CAN_Filter_FIFO0);
}
void IO_Congig(void)
{
	// 端口配置
	// 配置 < 0:0-5v 1:0-20mA 2:DI+ 3:DI- >
	ECU.PIN_3.Type = 2; // 变速箱低速模式
	ECU.PIN_4.Type = 2; //
	ECU.PIN_5.Type = 2; //
	ECU.PIN_6.Type = 2; //
	ECU.PIN_7.Type = 2; //
	// PWM_DO_DI 	< 0:DO 1:PWM 2:DI >
	ECU.PIN_2.Type = 0;	 // 倒挡喇叭输出
	ECU.PIN_10.Type = 0; // 四轮转向阀
	ECU.PIN_11.Type = 0; // 蟹行转向阀
	ECU.PIN_12.Type = 0; //
	ECU.PIN_13.Type = 0; // 平衡阀切断阀
	ECU.PIN_25.Type = 0; //
	ECU.PIN_26.Type = 1; // 行走泵前进方向
	ECU.PIN_29.Type = 1; // 行走泵后退方向
	ECU.PIN_38.Type = 1; // 行走马达
	ECU.PIN_39.Type = 0; // 手柄喇叭输出
	// PI/DI		< 0:DI 1:PI >
	ECU.PIN_14.Type = 1; // 发动机转速传感器
	ECU.PIN_15.Type = 1; // 后桥马达转速传感器
	ECU.PIN_18.Type = 0; // 后桥接近开关
	ECU.PIN_19.Type = 0; // 前桥接近开关
	/*
	//DI/AI     <PIN_8,PIN_9,PIN_16,PIN_17>
	无需配置，采集AI程序自动转为DI
		ECU.PIN_16				//四轮转向模式开关
		ECU.PIN_17				//蟹行转向模式开关
	*/
	// FB/DO-/DI< u8 Type;//0:DO 1:DI 2:FB >
	ECU.PIN_23.Type = 1;					// 刹车信号
	ECU.PIN_24.Type = 1;					//
	ECU.PIN_30.Type = 1;					//
	ECU.PIN_31.Type = 1;					//
	ECU.PWM_25_26_Set_PI.Par_PI = 200;		//
	ECU.PWM_2_29_38_39_Set_PI.Par_PI = 200; //
}
// ##########################2.主循环##################################
void KOP_Cycle(void)
{
	static u8 MID1, MID2, MID3, MID4, MID5, MID6, MID7, MID8, MID9, MID10, MID11, MID12, MID13, MID14, MID15, MID16;
	static u8 XIE_SI, Si_Xie_Liang;
	static u16 /*i, j, k,*/ n, SL_count, Boom_BH_count, YQ_Xie_count, YQ_Si_count, YQ_count;
	static u8 Boom_BH;
	static u16 DW_YS_count, DW_YS_count1; // 换挡延时计数
	static u8 DW_YS;
	static u8 DW_YS1;											// 换挡延时计数
	u16 Valve_Current;											// 泵阀电流值
	u16 Valve_Current_MD;										// 马达阀电流值
	u16 Valve_Current_Boom_UP;									// 大臂升电流值
	u16 Valve_Current_Boom_DOWN /*, Valve_Current_Boom_DOWN1*/; // 大臂降电流值
	u16 Valve_Current_Boom_Shen;								// 大臂伸出电流值
	u16 Valve_Current_Boom_Suo;									// 大臂缩回电流值
	u16 Valve_Current_FanDou;									// 翻斗电流值
	u16 Valve_Current_ShouDou;									// 收斗电流值
	u16 Valve_Current_FuJu_a;									// 辅具a电流值
	u16 Valve_Current_FuJu_b;									// 辅具b电流值
	// u8 Eng_Speed_BH;	  // 发动机转速保护
	u16 GBD_X1, GBD_Y1, GBD_X2, GBD_Y2, GBD_X3, GBD_Y3, GBD_X4, GBD_Y4, GBD_X5, GBD_Y5;
	u16 GBR_X1, GBR_Y1, GBR_X2, GBR_Y2, GBR_X3, GBR_Y3, GBR_X4, GBR_Y4, GBR_X5, GBR_Y5;
	u16 GM_X1, GM_Y1, GM_X2, GM_Y2, GM_X3, GM_Y3, GM_X4, GM_Y4;

	double GBD_k1, GBD_b1, GBD_k2, GBD_b2, GBD_k3, GBD_b3, GBD_k4, GBD_b4; // 高速泵前进电磁阀曲线斜率
	double GBR_k1, GBR_b1, GBR_k2, GBR_b2, GBR_k3, GBR_b3, GBR_k4, GBR_b4; // 高速泵后退电磁阀曲线斜率
	double GM_k1, GM_b1, GM_k2, GM_b2, GM_k3, GM_b3;					   // 高速泵后退电磁阀曲线斜率

	//===================输入采集========================
	WuGui_MoShi = ECU.PIN_3.DI;
	SiLun_MoShi_SB = ECU.PIN_16.DI;
	XieXing_MoShi_SB = ECU.PIN_17.DI;
	HouQiao_SB_1 = ECU.PIN_18.DI;
	QianQiao_SB_1 = ECU.PIN_19.DI;
	Eng_SP_Freq = ECU.PIN_14.PI;
	Motor_SP_Freq = ECU.PIN_15.PI;
	ShaChe_XH = ECU.PIN_23.DI;
	System_Voltage = ECU.Volt_Sys * 100;
	YouGanQiang_YaLi_mA = (Rx_281[1] << 8) | Rx_281[0];
	WuGanQiang_YaLi_mA = (Rx_281[3] << 8) | Rx_281[2];
	ShouBing_X_mV = (Rx_281[5] << 8) | Rx_281[4];
	ShouBing_Y_mV = (Rx_281[7] << 8) | Rx_281[6];
	DangWei_QianHou_mV = (Rx_282[1] << 8) | Rx_282[0];
	ShenSuo_BL_mV = (Rx_282[3] << 8) | Rx_282[2];
	FuJu_BL_mV = (Rx_282[5] << 8) | Rx_282[4];
	DangWei_N = (Rx_282[6] >> 0) & 1;
	DangWei_M = (Rx_282[6] >> 1) & 1;
	CunJin_BUT = (Rx_282[6] >> 2) & 1;
	// ######################四轮转向逻辑###########################################
	// 越权模式
	if (R_Trig(XieXing_MoShi_SB, &MID11))
	{
		ShouDong_YQ = 1;
		YQ_Xie_count++;
	}
	if (YQ_Xie_count == 3)
	{
		YQ_Xie = 1;
	}
	if (YQ_Xie)
	{
		YQ_Xie = 0;
		ECU.FeEL[11] = 2;
		YQ_Xie_count = 0;
	}

	if (R_Trig(SiLun_MoShi_SB, &MID12))
	{
		ShouDong_YQ = 1;
		YQ_Si_count++;
	}
	if (YQ_Si_count == 3)
	{
		YQ_Si = 1;
	}
	if (YQ_Si)
	{
		YQ_Si = 0;
		ECU.FeEL[11] = 1;
		YQ_Si_count = 0;
	}

	if (ShouDong_YQ)
	{
		YQ_count++;
		if (YQ_count == 300)
		{
			ShouDong_YQ = 0;
			YQ_count = 0;
			YQ_Si_count = 0;
			YQ_Xie_count = 0;
		}
	}

	// 正常模式
	if (R_Trig(XieXing_MoShi_SB, &MID1) == 1 || R_Trig(SiLun_MoShi_SB, &MID2) == 1)
	{
		XIE_SI = 1;
	}
	if (XIE_SI == 1)
	{
		if (HouQiao_SB_1 == 1)
		{
			XieXing_Fa = 0;
			SiLun_Fa = 0;
			XIE_SI = 0;
		}
	}
	// 四轮模式切换逻辑
	if (SiLun_MoShi_SB == 1)
	{
		if (ECU.FeEL[11] == 1)
		{
			SiLun_Fa = 1;
			XieXing_Fa = 0;
		}
		else if (ECU.FeEL[11] == 2)
		{
			if (HouQiao_SB_1 != 1)
			{
				SiLun_Fa = 0;
				XieXing_Fa = 1;
			}
			else
			{
				SiLun_Fa = 0;
				XieXing_Fa = 0;
			}
		}
		if (QianQiao_SB_1 == 1 && HouQiao_SB_1 == 1)
		{
			ECU.FeEL[11] = 1; // 四轮转向模式掉电保存
			SiLun_Fa = 1;
			XieXing_Fa = 0;
		}
	}
	// 蟹行模式切换逻辑
	if (XieXing_MoShi_SB == 1)
	{
		if (ECU.FeEL[11] == 2)
		{
			SiLun_Fa = 0;
			XieXing_Fa = 1;
		}
		else if (ECU.FeEL[11] == 1)
		{
			if (HouQiao_SB_1 != 1)
			{
				SiLun_Fa = 1;
				XieXing_Fa = 0;
			}
			else
			{
				SiLun_Fa = 0;
				XieXing_Fa = 0;
			}
		}
		if (QianQiao_SB_1 == 1 && HouQiao_SB_1 == 1)
		{
			ECU.FeEL[11] = 2; // 蟹行转向模式掉电保存
			XieXing_Fa = 1;
			SiLun_Fa = 0;
		}
	}
	if (SiLun_MoShi_SB == 0 && XieXing_MoShi_SB == 0)
	{
		if (HouQiao_SB_1 == 0)
		{
			/*
			i++;
			if (i > 100)
				i = 1;
			if (i > 50)
				LP_LiangLun = 0;
			else
				LP_LiangLun = 1;
			*/
			if (ECU.FeEL[11] == 1)
			{
				SiLun_Fa = 1;
			}
			if (ECU.FeEL[11] == 2)
			{
				XieXing_Fa = 1;
			}
		}
		if (HouQiao_SB_1 == 1)
		{
			SiLun_Fa = 0;
			XieXing_Fa = 0;
			Si_Xie_Liang = 1;
		}
		if (Si_Xie_Liang == 1)
		{
			SL_count++;
		}
		if (SL_count >= 50)
		{
			SL_count = 0;
			if (HouQiao_SB_1 == 1)
			{
				SiLun_Fa = 0;
				XieXing_Fa = 0;
				ECU.FeEL[11] = 0;
				Si_Xie_Liang = 0;
			}
			else
			{
				if (ECU.FeEL[11] == 1)
				{
					SiLun_Fa = 1;
				}
				if (ECU.FeEL[11] == 2)
				{
					XieXing_Fa = 1;
				}
			}
		}
	}
	// 亮灯逻辑
	if (SiLun_MoShi_SB == 1)
	{
		if (SiLun_Fa != 1)
		{

			LP_SiLun = 0;
			LP_SiLun_S = 1;
			LP_XieXing = 0;
			LP_XieXing_S = 0;
			LP_LiangLun = 0;
			LP_LiangLun_S = 0;
			/*
			j++;
			if (j > 100)
				j = 1;
			if (j > 50)
				LP_SiLun = 0;
			else
				LP_SiLun = 1;
				*/
		}
		else
		{
			LP_SiLun = 1;
			LP_SiLun_S = 0;
			LP_XieXing = 0;
			LP_XieXing_S = 0;
			LP_LiangLun = 0;
			LP_LiangLun_S = 0;
		}
	}
	else if (XieXing_MoShi_SB == 1)
	{
		if (XieXing_Fa != 1)
		{
			LP_SiLun = 0;
			LP_SiLun_S = 0;
			LP_XieXing = 0;
			LP_XieXing_S = 1;
			LP_LiangLun = 0;
			LP_LiangLun_S = 0;
			/*
			k++;
			if (k > 100)
				k = 1;
			if (k > 50)
				LP_XieXing = 0;
			else
				LP_XieXing = 1;
				*/
		}
		else
		{
			LP_XieXing = 1;
			LP_XieXing_S = 0;
			LP_SiLun = 0;
			LP_SiLun_S = 0;
			LP_LiangLun = 0;
			LP_LiangLun_S = 0;
		}
	}
	else
	{
		if (SiLun_Fa || XieXing_Fa)
		{
			LP_XieXing = 0;
			LP_XieXing_S = 0;
			LP_SiLun = 0;
			LP_SiLun_S = 0;
			LP_LiangLun = 0;
			LP_LiangLun_S = 1;
		}
		else
		{
			LP_XieXing = 0;
			LP_XieXing_S = 0;
			LP_SiLun = 0;
			LP_SiLun_S = 0;
			LP_LiangLun = 1;
			LP_LiangLun_S = 0;
		}
	}

	// ######################液压行走逻辑###########################################
	if (DangWei_N || R_Trig(System_Voltage > 0, &MID6))
	{
		DW_QianJin = 0;
		DW_HouTui = 0;
		DW_YS = 0;
		DW_YS1 = 0;
		DW_YS_count = 0;
		DW_YS_count1 = 0;
	}
	if (R_Trig(DangWei_QianHou_mV > 4000, &MID4))
	{
		// 挂入前进挡时，如果此时在后退档上，进入延时状态
		if (DW_HouTui)
		{
			DW_QianJin = 0;
			DW_HouTui = 0;
			DW_YS = 1;
		}
		else
		{
			DW_QianJin = 1;
			DW_HouTui = 0;
		}
	}
	else if (R_Trig(DangWei_QianHou_mV < 1000, &MID5))
	{
		if (DW_QianJin)
		{
			DW_QianJin = 0;
			DW_HouTui = 0;
			DW_YS1 = 1;
		}
		else
		{
			DW_QianJin = 0;
			DW_HouTui = 1;
		}
	}
	else if (DangWei_QianHou_mV > 4900 || DangWei_QianHou_mV < 300)
	{
		DW_QianJin = 0;
		DW_HouTui = 0;
		DW_YS = 0;
		DW_YS1 = 0;
		DW_YS_count = 0;
		DW_YS_count1 = 0;
	}

	if (DW_YS)
	{
		DW_YS_count++;
		// 延时800mm后，前进挡挂入
		if (DW_YS_count == 60)
		{
			DW_YS = 0;
			DW_YS_count = 0;
			DW_QianJin = 1;
			DW_HouTui = 0;
		}
	}
	if (DW_YS1)
	{
		DW_YS_count1++;

		// 延时00mm后，后退挡挂入
		if (DW_YS_count1 == 60)
		{
			DW_YS1 = 0;
			DW_YS_count1 = 0;
			DW_QianJin = 0;
			DW_HouTui = 1;
		}
	}

	if (R_Trig(Eng_Speed > 0, &MID7))
	{
		DW_QianJin = 0;
		DW_HouTui = 0;
		DW_YS = 0;
		DW_YS1 = 0;
		DW_YS_count = 0;
		DW_YS_count1 = 0;
	}
	if (DiSu_MoShi)
	{
		if (DW_QianJin)
		{
			D1 = 1;
			D2 = 0;
			N = 0;
			R1 = 0;
			R2 = 0;
		}
		else if (DW_HouTui)
		{
			D1 = 0;
			D2 = 0;
			N = 0;
			R1 = 1;
			R2 = 0;
		}
		else
		{
			D1 = 0;
			D2 = 0;
			N = 1;
			R1 = 0;
			R2 = 0;
		}
	}
	else
	{
		if (DW_QianJin)
		{
			D1 = 0;
			D2 = 1;
			N = 0;
			R1 = 0;
			R2 = 0;
		}
		else if (DW_HouTui)
		{
			D1 = 0;
			D2 = 0;
			N = 0;
			R1 = 0;
			R2 = 1;
		}
		else
		{
			D1 = 0;
			D2 = 0;
			N = 1;
			R1 = 0;
			R2 = 0;
		}
	}

	/*低速模式*/
	if (R_Trig(DangWei_M, &MID9))
	{
		if (DiSu_MoShi == 1)
		{
			DiSu_MoShi = 0;
		}
		else
		{
			DiSu_MoShi = 1;
		}
	}
	/* 发动机转速斜坡定义 */
	Eng_Speed_JS = PI_Filter((Eng_SP_Freq * 60) / 130);
	if (CunJin_BUT)
	{
		if (Eng_Speed_JS >= 1250)
		{
			Eng_Speed_JS = 1250;
		}
	}

	// Eng_Speed_JS = (Eng_SP_Freq * 60) / 130;

	if (R_Trig(DW_QianJin, &MID13) || R_Trig(DW_HouTui, &MID14) || R_Trig(ShaChe_XH, &MID16) /* || F_Trig(CunJin_BUT, &MID3) */)
	{
		HuanDang_XH = 1;
	}
	if (R_Trig(Eng_Speed_JS > 1100, &MID15))
	{
		HuanDang_XH = 0;
	}
	if (WuGui_MoShi)
	{
		Beng_DecTm = Beng_DecTm_WG;
		/* 新增刹车斜坡信号 */
		if (ShaChe_XH )
		{
			Eng_Speed = (Dec_Option_32(Eng_Speed_JS * 1000, 15000, 5, ARR8)) / 1000;
			if (Eng_Speed <= 950)
			{
				Eng_Speed = 90;
			}
		}
		else
		{
			Eng_Speed = (Dec_Option_32(Eng_Speed_JS * 1000, Beng_DecTm, 10, ARR8)) / 1000;
		}

		Motor_IncTm = Motor_IncTm_WG;
		Motor_DecTm = Motor_DecTm_WG;
	}
	else
	{
		Beng_IncTm = Beng_IncTm_TZ;
		Eng_Speed1 = (Inc_Option(Eng_Speed_JS * 1000, Beng_IncTm, 10, ARR3)) / 1000;
		/* 新增刹车斜坡信号 */
		if (ShaChe_XH )
		{
			Eng_Speed = (Dec_Option_32(Eng_Speed1 * 1000, 15000, 5, ARR7)) / 1000;
			if (Eng_Speed <= 950)
			{
				Eng_Speed = 90;
			}
		}
		else
		{
			Eng_Speed = (Dec_Option_32(Eng_Speed1 * 1000, 1500, 10, ARR7)) / 1000;
		}
		Motor_IncTm = Motor_IncTm_TZ;
		Motor_DecTm = Motor_DecTm_TZ;
	}
	Eng_Speed_MD1 = (Inc_Option(Eng_Speed_JS * 1000, Motor_IncTm, 10, ARR5)) / 1000;
	Eng_Speed_MD = (Dec_Option_32(Eng_Speed_MD1 * 1000, Motor_DecTm, 10, ARR6)) / 1000;
	/* 马达转速及车速计算 */
	Motor_Speed = (Motor_SP_Freq * 60) / 67;
	if (WuGui_MoShi)
	{
		XingChe_SuDu = Motor_Speed / 3.92 / 12.923 * 3.14 * 1.15 * 60 / 100;
	}
	else
	{
		XingChe_SuDu = Motor_Speed / 0.7874 / 12.923 * 3.14 * 1.15 * 60 / 100;
	}

	//=====================曲线X\Y定义=============================
	// 曲线初始化
	if (ECU.FeEL[0] != 13245)
	{
		ECU.FeEL[0] = 13245;
		// 泵前进曲线初始化
		ECU.FeEL[21] = 900;
		ECU.FeEL[22] = 430;
		ECU.FeEL[23] = 1078;
		ECU.FeEL[24] = 556;
		ECU.FeEL[25] = 1572;
		ECU.FeEL[26] = 695;
		ECU.FeEL[27] = 1973;
		ECU.FeEL[28] = 809;
		ECU.FeEL[29] = 2350;
		ECU.FeEL[30] = 1150;
		// 泵后退曲线初始化
		ECU.FeEL[31] = 900;
		ECU.FeEL[32] = 410;
		ECU.FeEL[33] = 1083;
		ECU.FeEL[34] = 537;
		ECU.FeEL[35] = 1572;
		ECU.FeEL[36] = 681;
		ECU.FeEL[37] = 1973;
		ECU.FeEL[38] = 809;
		ECU.FeEL[39] = 2291;
		ECU.FeEL[40] = 950;
		// 马达曲线初始化
		ECU.FeEL[41] = 1033;
		ECU.FeEL[42] = 40;
		ECU.FeEL[43] = 1394;
		ECU.FeEL[44] = 386;
		ECU.FeEL[45] = 1874;
		ECU.FeEL[46] = 742;
		ECU.FeEL[47] = 2300;
		ECU.FeEL[48] = 900;
		// 斜坡曲线初始化
		ECU.FeEL[49] = 2500;
		ECU.FeEL[50] = 1200;
		ECU.FeEL[51] = 800;
		ECU.FeEL[52] = 600;
		ECU.FeEL[53] = 3800;
		ECU.FeEL[54] = 1200;
		ECU.FeEL[55] = 500;
	}

	GBD_X1 = ECU.FeEL[21];
	GBD_Y1 = ECU.FeEL[22];
	GBD_X2 = ECU.FeEL[23];
	GBD_Y2 = ECU.FeEL[24];
	GBD_X3 = ECU.FeEL[25];
	GBD_Y3 = ECU.FeEL[26];
	GBD_X4 = ECU.FeEL[27];
	GBD_Y4 = ECU.FeEL[28];
	GBD_X5 = ECU.FeEL[29];
	GBD_Y5 = ECU.FeEL[30];

	GBR_X1 = ECU.FeEL[31];
	GBR_Y1 = ECU.FeEL[32];
	GBR_X2 = ECU.FeEL[33];
	GBR_Y2 = ECU.FeEL[34];
	GBR_X3 = ECU.FeEL[35];
	GBR_Y3 = ECU.FeEL[36];
	GBR_X4 = ECU.FeEL[37];
	GBR_Y4 = ECU.FeEL[38];
	GBR_X5 = ECU.FeEL[39];
	GBR_Y5 = ECU.FeEL[40];

	GM_X1 = ECU.FeEL[41];
	GM_Y1 = ECU.FeEL[42];
	GM_X2 = ECU.FeEL[43];
	GM_Y2 = ECU.FeEL[44];
	GM_X3 = ECU.FeEL[45];
	GM_Y3 = ECU.FeEL[46];
	GM_X4 = ECU.FeEL[47];
	GM_Y4 = ECU.FeEL[48];

	Beng_IncTm_TZ = ECU.FeEL[49];
	Beng_DecTm_WG = ECU.FeEL[50];
	Motor_IncTm_TZ = ECU.FeEL[51];
	Motor_DecTm_TZ = ECU.FeEL[52];
	Motor_IncTm_WG = ECU.FeEL[53];
	Motor_DecTm_WG = ECU.FeEL[54];
	CunJin_Current = ECU.FeEL[55];

	//=====================曲线斜率计算=============================
	GBD_k1 = (double)(GBD_Y2 - GBD_Y1) / (GBD_X2 - GBD_X1);
	GBD_b1 = (double)GBD_Y2 - (GBD_k1 * GBD_X2);
	GBD_k2 = (double)(GBD_Y3 - GBD_Y2) / (GBD_X3 - GBD_X2);
	GBD_b2 = (double)GBD_Y3 - (GBD_k2 * GBD_X3);
	GBD_k3 = (double)(GBD_Y4 - GBD_Y3) / (GBD_X4 - GBD_X3);
	GBD_b3 = (double)GBD_Y4 - (GBD_k3 * GBD_X4);
	GBD_k4 = (double)(GBD_Y5 - GBD_Y4) / (GBD_X5 - GBD_X4);
	GBD_b4 = (double)GBD_Y5 - (GBD_k4 * GBD_X5);

	GBR_k1 = (double)(GBR_Y2 - GBR_Y1) / (GBR_X2 - GBR_X1);
	GBR_b1 = (double)GBR_Y2 - (GBR_k1 * GBR_X2);
	GBR_k2 = (double)(GBR_Y3 - GBR_Y2) / (GBR_X3 - GBR_X2);
	GBR_b2 = (double)GBR_Y3 - (GBR_k2 * GBR_X3);
	GBR_k3 = (double)(GBR_Y4 - GBR_Y3) / (GBR_X4 - GBR_X3);
	GBR_b3 = (double)GBR_Y4 - (GBR_k3 * GBR_X4);
	GBR_k4 = (double)(GBR_Y5 - GBR_Y4) / (GBR_X5 - GBR_X4);
	GBR_b4 = (double)GBR_Y5 - (GBR_k4 * GBR_X5);

	GM_k1 = (double)(GM_Y2 - GM_Y1) / (GM_X2 - GM_X1);
	GM_b1 = (double)GM_Y2 - (GM_k1 * GM_X2);
	GM_k2 = (double)(GM_Y3 - GM_Y2) / (GM_X3 - GM_X2);
	GM_b2 = (double)GM_Y3 - (GM_k2 * GM_X3);
	GM_k3 = (double)(GM_Y4 - GM_Y3) / (GM_X4 - GM_X3);
	GM_b3 = (double)GM_Y4 - (GM_k3 * GM_X4);

	if (DW_QianJin == 1)
	{
		if (CunJin_BUT)
		{
			if (Eng_Speed >= 1200)
			{
				Valve_Current = 550;
			}
			else
			{
				Valve_Current = 0;
			}
		}
		else
		{
			if (Eng_Speed < 100)
			{
				Valve_Current = 0;
			}
			else if (Eng_Speed >= 100 && Eng_Speed <= GBD_X1)
			{
				Valve_Current = GBD_Y1;
			}
			else if (Eng_Speed > GBD_X1 && Eng_Speed <= GBD_X2) // 第一段曲线
			{
				Valve_Current = GBD_k1 * Eng_Speed + GBD_b1;
			}
			else if (Eng_Speed > GBD_X2 && Eng_Speed <= GBD_X3) // 第二段曲线
			{
				Valve_Current = GBD_k2 * Eng_Speed + GBD_b2;
			}
			else if (Eng_Speed > GBD_X3 && Eng_Speed <= GBD_X4) // 第三段曲线
			{
				Valve_Current = GBD_k3 * Eng_Speed + GBD_b3;
			}
			else if (Eng_Speed > GBD_X4 && Eng_Speed <= GBD_X5) // 第四段曲线
			{
				Valve_Current = GBD_k4 * Eng_Speed + GBD_b4;
			}
			else if (Eng_Speed > GBD_X5)
			{
				Valve_Current = GBD_Y5;
			}
		}
		XingZou_QJ_Fa = (32767 * Valve_Current * CoilResistance) / System_Voltage;
		XingZou_HT_Fa = 0;
	}
	else if (DW_HouTui == 1)
	{
		if (Eng_Speed < 100)
		{
			Valve_Current = 0;
		}
		else if (Eng_Speed <= GBR_X1)
		{
			Valve_Current = GBR_Y1;
		}

		else if (Eng_Speed > GBR_X1 && Eng_Speed <= GBR_X2) // 第一段曲线
		{
			Valve_Current = GBR_k1 * Eng_Speed + GBR_b1;
		}
		else if (Eng_Speed > GBR_X2 && Eng_Speed <= GBR_X3) // 第二段曲线
		{
			Valve_Current = GBR_k2 * Eng_Speed + GBR_b2;
		}
		else if (Eng_Speed > GBR_X3 && Eng_Speed <= GBR_X4) // 第三段曲线
		{
			Valve_Current = GBR_k3 * Eng_Speed + GBR_b3;
		}
		else if (Eng_Speed > GBR_X4 && Eng_Speed <= GBR_X5) // 第四段曲线
		{
			Valve_Current = GBR_k4 * Eng_Speed + GBR_b4;
		}
		else if (Eng_Speed > GBR_X5)
		{
			Valve_Current = GBR_Y5;
		}
		XingZou_QJ_Fa = 0;
		XingZou_HT_Fa = (32767 * Valve_Current * CoilResistance) / System_Voltage;
	}
	else
	{
		Valve_Current = 0;
		XingZou_QJ_Fa = 0;
		XingZou_HT_Fa = 0;
	}
	if (DiSu_MoShi == 1 || CunJin_BUT == 1)
	{
		Valve_Current_MD = 0;
	}
	else
	{
		if (Eng_Speed_MD <= GM_X1)
		{
			Valve_Current_MD = 0;
		}
		else if (Eng_Speed_MD > GM_X1 && Eng_Speed_MD <= GM_X2)
		{
			Valve_Current_MD = GM_k1 * Eng_Speed_MD + GM_b1;
		}
		else if (Eng_Speed_MD > GM_X2 && Eng_Speed_MD <= GM_X3)
		{
			Valve_Current_MD = GM_k2 * Eng_Speed_MD + GM_b2;
		}
		else if (Eng_Speed_MD > GM_X3 && Eng_Speed_MD <= GM_X4)
		{
			Valve_Current_MD = GM_k3 * Eng_Speed_MD + GM_b3;
		}
		else if (Eng_Speed_MD > GM_X4)
		{
			Valve_Current_MD = GM_Y4;
		}
	}

	if (R_Trig(DangWei_N, &MID8))
	{
		XingZou_QJ_Fa = 0;
		XingZou_HT_Fa = 0;
		Valve_Current_MD = GM_Y4;
	}

	//===================上装操作========================
	/*大臂变幅*/
	if (ShouBing_Y_mV <= 2300 && ShouBing_Y_mV > 100)
	{
		Valve_Current_Boom_UP = 0;
		if (ShouBing_Y_mV <= 1700 && ShouBing_Y_mV > 100)
		{
			Valve_Current_Boom_DOWN = SCALE_U16(Dec_Option(ShouBing_Y_mV, 3, 10, ARR4), 2300, 500, 380, 750);
		}
		else
		{
			Valve_Current_Boom_DOWN = SCALE_U16(ShouBing_Y_mV, 2300, 500, 380, 750);
		}
	}
	else if (ShouBing_Y_mV >= 2750 & ShouBing_Y_mV < 4900)
	{
		Valve_Current_Boom_UP = SCALE_U16(ShouBing_Y_mV, 2750, 4600, 320, 700);
		Valve_Current_Boom_DOWN = 0;
	}
	else
	{
		Valve_Current_Boom_UP = 0;
		Valve_Current_Boom_DOWN = 0;
	}
	Boom_UP = (32767 * Valve_Current_Boom_UP * 21) / System_Voltage;
	Boom_DOWN = (32767 * Valve_Current_Boom_DOWN * 21) / System_Voltage;
	// 大臂下降时打开平衡阀切断阀
	if (Valve_Current_Boom_DOWN > 400)
	{
		PingHengFa_QD = 1;
	}
	else
	{
		PingHengFa_QD = 0;
	}

	if (R_Trig(Boom_UP < 10 || Boom_DOWN < 10, &MID10))
	{
		Boom_BH = 1;
	}
	if (Boom_BH)
	{
		Boom_UP = 0;
		Boom_DOWN = 0;
		Boom_BH_count++;
		if (Boom_BH_count == 100)
		{
			Boom_BH = 0;
			Boom_BH_count = 0;
		}
	}

	/*大臂伸缩*/
	if (ShenSuo_BL_mV <= 2400 && ShenSuo_BL_mV > 100)
	{
		Valve_Current_Boom_Shen = 0;
		Valve_Current_Boom_Suo = SCALE_U16(ShenSuo_BL_mV, 2400, 500, 450, 800);
	}
	else if (ShenSuo_BL_mV >= 2700 && ShenSuo_BL_mV < 4900)
	{
		Valve_Current_Boom_Shen = SCALE_U16(ShenSuo_BL_mV, 2700, 4600, 450, 800);
		Valve_Current_Boom_Suo = 0;
	}
	else
	{
		Valve_Current_Boom_Shen = 0;
		Valve_Current_Boom_Suo = 0;
	}
	Boom_Shen = (32767 * Valve_Current_Boom_Shen * 21) / System_Voltage;
	Boom_Suo = (32767 * Valve_Current_Boom_Suo * 21) / System_Voltage;
	/*翻斗收斗*/
	if (ShouBing_X_mV <= 2300 && ShouBing_X_mV > 100)
	{
		Valve_Current_FanDou = 0;
		Valve_Current_ShouDou = SCALE_U16(ShouBing_X_mV, 2400, 500, 380, 680);
	}
	else if (ShouBing_X_mV >= 2750 && ShouBing_X_mV < 4900)
	{
		Valve_Current_FanDou = SCALE_U16(ShouBing_X_mV, 2750, 4500, 380, 630);
		Valve_Current_ShouDou = 0;
	}
	else
	{
		Valve_Current_FanDou = 0;
		Valve_Current_ShouDou = 0;
	}
	FanDou_UP = (32767 * Valve_Current_ShouDou * 21) / System_Voltage;
	FanDou_DOWN = (32767 * Valve_Current_FanDou * 21) / System_Voltage;
	/*辅具*/
	if (FuJu_BL_mV <= 2400 && FuJu_BL_mV > 100)
	{
		Valve_Current_FuJu_a = 0;
		Valve_Current_FuJu_b = SCALE_U16(FuJu_BL_mV, 2400, 500, 450, 750);
	}
	else if (FuJu_BL_mV >= 2700 && FuJu_BL_mV < 4900)
	{
		Valve_Current_FuJu_a = SCALE_U16(FuJu_BL_mV, 2700, 4600, 450, 750);
		Valve_Current_FuJu_b = 0;
	}
	else
	{
		Valve_Current_FuJu_a = 0;
		Valve_Current_FuJu_b = 0;
	}
	FuJu_Valve_a = (32767 * Valve_Current_FuJu_a * 21) / System_Voltage;
	FuJu_Valve_b = (32767 * Valve_Current_FuJu_b * 21) / System_Voltage;

	//===================输出刷新========================
	ECU.PIN_2.DO = DW_HouTui;
	ECU.PIN_10.DO = SiLun_Fa;
	ECU.PIN_11.DO = XieXing_Fa;
	ECU.PIN_12.DO = LP_LiangLun;
	ECU.PIN_13.DO = LP_SiLun;
	ECU.PIN_25.DO = LP_XieXing;
	ECU.PIN_26.PWM = XingZou_QJ_Fa;
	ECU.PIN_29.PWM = XingZou_HT_Fa;
	ECU.PIN_38.PWM = (32767 * Valve_Current_MD * CoilResistance) / System_Voltage;
	Tx_181[0] = (FuJu_Valve_a >> 0) & 0xFF;
	Tx_181[1] = (FuJu_Valve_a >> 8) & 0xFF;
	Tx_181[2] = (FuJu_Valve_b >> 0) & 0xFF;
	Tx_181[3] = (FuJu_Valve_b >> 8) & 0xFF;
	Tx_181[4] = (Boom_Shen >> 0) & 0xFF;
	Tx_181[5] = (Boom_Shen >> 8) & 0xFF;
	Tx_181[6] = (Boom_Suo >> 0) & 0xFF;
	Tx_181[7] = (Boom_Suo >> 8) & 0xFF;

	Tx_182[0] = (Boom_UP >> 0) & 0xFF;
	Tx_182[1] = (Boom_UP >> 8) & 0xFF;
	Tx_182[2] = (Boom_DOWN >> 0) & 0xFF;
	Tx_182[3] = (Boom_DOWN >> 8) & 0xFF;
	Tx_182[4] = (FanDou_UP >> 0) & 0xFF;
	Tx_182[5] = (FanDou_UP >> 8) & 0xFF;
	Tx_182[6] = (FanDou_DOWN >> 0) & 0xFF;
	Tx_182[7] = (FanDou_DOWN >> 8) & 0xFF;

	Tx_SL[0] = LP_LiangLun + LP_LiangLun_S * 2 + LP_SiLun * 4 + LP_SiLun_S * 8 + LP_XieXing * 16 + LP_XieXing_S * 32;
	Tx_SL[1] = D1 + D2 * 2 + N * 4 + R1 * 8 + R2 * 16;
	Tx_SL[2] = 0;
	Tx_SL[3] = 0;
	Tx_SL[4] = 0;
	Tx_SL[5] = 0;
	Tx_SL[6] = 0;
	Tx_SL[7] = 0;

	//=======================CAN========================

	Tx_101[0] = ECU.FeEL[21] % 256;
	Tx_101[1] = ECU.FeEL[21] / 256;
	Tx_101[2] = ECU.FeEL[22] % 256;
	Tx_101[3] = ECU.FeEL[22] / 256;
	Tx_101[4] = ECU.FeEL[23] % 256;
	Tx_101[5] = ECU.FeEL[23] / 256;
	Tx_101[6] = ECU.FeEL[24] % 256;
	Tx_101[7] = ECU.FeEL[24] / 256;

	Tx_102[0] = ECU.FeEL[25] % 256;
	Tx_102[1] = ECU.FeEL[25] / 256;
	Tx_102[2] = ECU.FeEL[26] % 256;
	Tx_102[3] = ECU.FeEL[26] / 256;
	Tx_102[4] = ECU.FeEL[27] % 256;
	Tx_102[5] = ECU.FeEL[27] / 256;
	Tx_102[6] = ECU.FeEL[28] % 256;
	Tx_102[7] = ECU.FeEL[28] / 256;

	Tx_103[0] = ECU.FeEL[29] % 256;
	Tx_103[1] = ECU.FeEL[29] / 256;
	Tx_103[2] = ECU.FeEL[30] % 256;
	Tx_103[3] = ECU.FeEL[30] / 256;
	Tx_103[4] = System_Voltage % 256;
	Tx_103[5] = System_Voltage / 256;
	Tx_103[6] = Eng_Speed % 256;
	Tx_103[7] = Eng_Speed / 256;

	Tx_104[0] = Valve_Current % 256;
	Tx_104[1] = Valve_Current / 256;
	Tx_104[2] = XingZou_QJ_Fa % 256;
	Tx_104[3] = XingZou_QJ_Fa / 256;
	Tx_104[4] = XingZou_HT_Fa % 256;
	Tx_104[5] = XingZou_HT_Fa / 256;
	Tx_104[6] = Eng_Speed_JS % 256;
	Tx_104[7] = Eng_Speed_JS / 256;

	Tx_105[0] = ECU.FeEL[31] % 256;
	Tx_105[1] = ECU.FeEL[31] / 256;
	Tx_105[2] = ECU.FeEL[32] % 256;
	Tx_105[3] = ECU.FeEL[32] / 256;
	Tx_105[4] = ECU.FeEL[33] % 256;
	Tx_105[5] = ECU.FeEL[33] / 256;
	Tx_105[6] = ECU.FeEL[34] % 256;
	Tx_105[7] = ECU.FeEL[34] / 256;

	Tx_106[0] = ECU.FeEL[35] % 256;
	Tx_106[1] = ECU.FeEL[35] / 256;
	Tx_106[2] = ECU.FeEL[36] % 256;
	Tx_106[3] = ECU.FeEL[36] / 256;
	Tx_106[4] = ECU.FeEL[37] % 256;
	Tx_106[5] = ECU.FeEL[37] / 256;
	Tx_106[6] = ECU.FeEL[38] % 256;
	Tx_106[7] = ECU.FeEL[38] / 256;

	Tx_107[0] = ECU.FeEL[39] % 256;
	Tx_107[1] = ECU.FeEL[39] / 256;
	Tx_107[2] = ECU.FeEL[40] % 256;
	Tx_107[3] = ECU.FeEL[40] / 256;
	Tx_107[4] = Valve_Current_MD % 256;
	Tx_107[5] = Valve_Current_MD / 256;
	Tx_107[6] = 0;
	Tx_107[7] = 0;

	Tx_108[0] = ECU.FeEL[41] % 256;
	Tx_108[1] = ECU.FeEL[41] / 256;
	Tx_108[2] = ECU.FeEL[42] % 256;
	Tx_108[3] = ECU.FeEL[42] / 256;
	Tx_108[4] = ECU.FeEL[43] % 256;
	Tx_108[5] = ECU.FeEL[43] / 256;
	Tx_108[6] = ECU.FeEL[44] % 256;
	Tx_108[7] = ECU.FeEL[44] / 256;

	Tx_109[0] = ECU.FeEL[45] % 256;
	Tx_109[1] = ECU.FeEL[45] / 256;
	Tx_109[2] = ECU.FeEL[46] % 256;
	Tx_109[3] = ECU.FeEL[46] / 256;
	Tx_109[4] = ECU.FeEL[47] % 256;
	Tx_109[5] = ECU.FeEL[47] / 256;
	Tx_109[6] = ECU.FeEL[48] % 256;
	Tx_109[7] = ECU.FeEL[48] / 256;

	Tx_110[0] = ECU.FeEL[49] % 256;
	Tx_110[1] = ECU.FeEL[49] / 256;
	Tx_110[2] = ECU.FeEL[50] % 256;
	Tx_110[3] = ECU.FeEL[50] / 256;
	Tx_110[4] = ECU.FeEL[51] % 256;
	Tx_110[5] = ECU.FeEL[51] / 256;
	Tx_110[6] = ECU.FeEL[52] % 256;
	Tx_110[7] = ECU.FeEL[52] / 256;

	Tx_111[0] = ECU.FeEL[53] % 256;
	Tx_111[1] = ECU.FeEL[53] / 256;
	Tx_111[2] = ECU.FeEL[54] % 256;
	Tx_111[3] = ECU.FeEL[54] / 256;
	Tx_111[4] = 0;
	Tx_111[5] = 0;
	Tx_111[6] = 0;
	Tx_111[7] = 0;

	Tx_121[0] = DW_QianJin + DW_HouTui * 2 + SiLun_MoShi_SB * 4 + XieXing_MoShi_SB * 8 + QianQiao_SB_1 * 16 + HouQiao_SB_1 * 32 + ECU.PIN_10.DO * 64 + ECU.PIN_11.DO * 128;
	Tx_121[1] = WuGui_MoShi + ShaChe_XH * 2;
	Tx_121[2] = 0;
	Tx_121[3] = 0;
	Tx_121[4] = 0;
	Tx_121[5] = 0;
	Tx_121[6] = 0;
	Tx_121[7] = 0;

	Tx_10A[0] = (ShouBing_X_mV >> 0) & 0xFF;
	Tx_10A[1] = (ShouBing_X_mV >> 8) & 0xFF;
	Tx_10A[2] = (ShouBing_Y_mV >> 0) & 0xFF;
	Tx_10A[3] = (ShouBing_Y_mV >> 8) & 0xFF;
	Tx_10A[4] = (ShenSuo_BL_mV >> 0) & 0xFF;
	Tx_10A[5] = (ShenSuo_BL_mV >> 8) & 0xFF;
	Tx_10A[6] = (FuJu_BL_mV >> 0) & 0xFF;
	Tx_10A[7] = (FuJu_BL_mV >> 8) & 0xFF;

	Tx_10B[0] = (Valve_Current_Boom_UP >> 0) & 0xFF;
	Tx_10B[1] = (Valve_Current_Boom_UP >> 8) & 0xFF;
	Tx_10B[2] = (Valve_Current_Boom_DOWN >> 0) & 0xFF;
	Tx_10B[3] = (Valve_Current_Boom_DOWN >> 8) & 0xFF;
	Tx_10B[4] = (Valve_Current_Boom_Shen >> 0) & 0xFF;
	Tx_10B[5] = (Valve_Current_Boom_Shen >> 8) & 0xFF;
	Tx_10B[6] = (Valve_Current_Boom_Suo >> 0) & 0xFF;
	Tx_10B[7] = (Valve_Current_Boom_Suo >> 8) & 0xFF;

	Tx_10C[0] = (Valve_Current_FanDou >> 0) & 0xFF;
	Tx_10C[1] = (Valve_Current_FanDou >> 8) & 0xFF;
	Tx_10C[2] = (Valve_Current_ShouDou >> 0) & 0xFF;
	Tx_10C[3] = (Valve_Current_ShouDou >> 8) & 0xFF;
	Tx_10C[4] = (Valve_Current_FuJu_a >> 0) & 0xFF;
	Tx_10C[5] = (Valve_Current_FuJu_a >> 8) & 0xFF;
	Tx_10C[6] = (Valve_Current_FuJu_b >> 0) & 0xFF;
	Tx_10C[7] = (Valve_Current_FuJu_b >> 8) & 0xFF;

	// 计数函数
	n++;
	if (n == 10)
	{
		CAN_Send_Msg_FC(1, 0x101, CAN_Id_Standard, 8, Tx_101);
		CAN_Send_Msg_FC(1, 0x102, CAN_Id_Standard, 8, Tx_102);
		CAN_Send_Msg_FC(1, 0x103, CAN_Id_Standard, 8, Tx_103);
	}
	if (n == 20)
	{
		CAN_Send_Msg_FC(1, 0x104, CAN_Id_Standard, 8, Tx_104);
		CAN_Send_Msg_FC(1, 0x105, CAN_Id_Standard, 8, Tx_105);
		CAN_Send_Msg_FC(1, 0x106, CAN_Id_Standard, 8, Tx_106);
	}
	if (n == 30)
	{
		CAN_Send_Msg_FC(1, 0x107, CAN_Id_Standard, 8, Tx_107);
		CAN_Send_Msg_FC(1, 0x108, CAN_Id_Standard, 8, Tx_108);
		CAN_Send_Msg_FC(1, 0x109, CAN_Id_Standard, 8, Tx_109);
	}
	if (n == 40)
	{

		CAN_Send_Msg_FC(1, 0x110, CAN_Id_Standard, 8, Tx_110);
		CAN_Send_Msg_FC(1, 0x111, CAN_Id_Standard, 8, Tx_111);
		CAN_Send_Msg_FC(1, 0x121, CAN_Id_Standard, 8, Tx_121);
	}
	if (n == 50)
	{
		CAN_Send_Msg_FC(1, 0x10A, CAN_Id_Standard, 8, Tx_10A);
		CAN_Send_Msg_FC(1, 0x10B, CAN_Id_Standard, 8, Tx_10B);
		CAN_Send_Msg_FC(1, 0x10C, CAN_Id_Standard, 8, Tx_10C);
		n = 0;
	}
	CAN_Send_Msg_FC(2, 0x191, CAN_Id_Standard, 8, Tx_181);
	CAN_Send_Msg_FC(2, 0x192, CAN_Id_Standard, 8, Tx_182);
	CAN_Send_Msg_FC(2, 0x18FDDAD6, CAN_Id_Extended, 8, Tx_SL);
}
// #########################4.CAN1接收单元#############################
void KOP_CAN1_Event(void)
{
	static u32 ii;
	CanRxMsg RxMessage;
	CAN_Receive(CAN1, 0, &RxMessage);
	//
	if (RxMessage.StdId == 0x201)
	{
		for (ii = 0; ii < 8; ii++)
		{
			Rx_201[ii] = RxMessage.Data[ii];
			ECU.FeEL[21] = Rx_201[0] + Rx_201[1] * 256;
			ECU.FeEL[22] = Rx_201[2] + Rx_201[3] * 256;
			ECU.FeEL[23] = Rx_201[4] + Rx_201[5] * 256;
			ECU.FeEL[24] = Rx_201[6] + Rx_201[7] * 256;
		}
	}
	//
	if (RxMessage.StdId == 0x202)
	{
		for (ii = 0; ii < 8; ii++)
		{
			Rx_202[ii] = RxMessage.Data[ii];
			ECU.FeEL[25] = Rx_202[0] + Rx_202[1] * 256;
			ECU.FeEL[26] = Rx_202[2] + Rx_202[3] * 256;
			ECU.FeEL[27] = Rx_202[4] + Rx_202[5] * 256;
			ECU.FeEL[28] = Rx_202[6] + Rx_202[7] * 256;
		}
	}
	//
	if (RxMessage.StdId == 0x203)
	{
		for (ii = 0; ii < 8; ii++)
		{
			Rx_203[ii] = RxMessage.Data[ii];
			ECU.FeEL[29] = Rx_203[0] + Rx_203[1] * 256;
			ECU.FeEL[30] = Rx_203[2] + Rx_203[3] * 256;
		}
	}
	//
	if (RxMessage.StdId == 0x204)
	{
		for (ii = 0; ii < 8; ii++)
		{
			Rx_204[ii] = RxMessage.Data[ii];
			ECU.FeEL[31] = Rx_204[0] + Rx_204[1] * 256;
			ECU.FeEL[32] = Rx_204[2] + Rx_204[3] * 256;
			ECU.FeEL[33] = Rx_204[4] + Rx_204[5] * 256;
			ECU.FeEL[34] = Rx_204[6] + Rx_204[7] * 256;
		}
	}
	//
	if (RxMessage.StdId == 0x205)
	{
		for (ii = 0; ii < 8; ii++)
		{
			Rx_205[ii] = RxMessage.Data[ii];
			ECU.FeEL[35] = Rx_205[0] + Rx_205[1] * 256;
			ECU.FeEL[36] = Rx_205[2] + Rx_205[3] * 256;
			ECU.FeEL[37] = Rx_205[4] + Rx_205[5] * 256;
			ECU.FeEL[38] = Rx_205[6] + Rx_205[7] * 256;
		}
	}
	//
	if (RxMessage.StdId == 0x206)
	{
		for (ii = 0; ii < 8; ii++)
		{
			Rx_206[ii] = RxMessage.Data[ii];
			ECU.FeEL[39] = Rx_206[0] + Rx_206[1] * 256;
			ECU.FeEL[40] = Rx_206[2] + Rx_206[3] * 256;
		}
	}
	//
	if (RxMessage.StdId == 0x207)
	{
		for (ii = 0; ii < 8; ii++)
		{
			Rx_207[ii] = RxMessage.Data[ii];
			ECU.FeEL[41] = Rx_207[0] + Rx_207[1] * 256;
			ECU.FeEL[42] = Rx_207[2] + Rx_207[3] * 256;
			ECU.FeEL[43] = Rx_207[4] + Rx_207[5] * 256;
			ECU.FeEL[44] = Rx_207[6] + Rx_207[7] * 256;
		}
	}
	//
	if (RxMessage.StdId == 0x208)
	{
		for (ii = 0; ii < 8; ii++)
		{
			Rx_208[ii] = RxMessage.Data[ii];
			ECU.FeEL[45] = Rx_208[0] + Rx_208[1] * 256;
			ECU.FeEL[46] = Rx_208[2] + Rx_208[3] * 256;
			ECU.FeEL[47] = Rx_208[4] + Rx_208[5] * 256;
			ECU.FeEL[48] = Rx_208[6] + Rx_208[7] * 256;
		}
	}
	//
	if (RxMessage.StdId == 0x209)
	{
		ECU.FeEL[49] = RxMessage.Data[0] + RxMessage.Data[1] * 256;
		ECU.FeEL[50] = RxMessage.Data[2] + RxMessage.Data[3] * 256;
		ECU.FeEL[51] = RxMessage.Data[4] + RxMessage.Data[5] * 256;
		ECU.FeEL[52] = RxMessage.Data[6] + RxMessage.Data[7] * 256;
	}
	//
	if (RxMessage.StdId == 0x20A)
	{
		ECU.FeEL[53] = RxMessage.Data[0] + RxMessage.Data[1] * 256;
		ECU.FeEL[54] = RxMessage.Data[2] + RxMessage.Data[3] * 256;
		ECU.FeEL[55] = RxMessage.Data[4] + RxMessage.Data[5] * 256;
	}
}
// #########################5.CAN2接收单元#############################
void KOP_CAN2_Event(void)
{
	static u8 i;
	CanRxMsg RxMessage;
	CAN_Receive(CAN2, 0, &RxMessage);
	if (RxMessage.StdId == 0x28A)
	{
		for (i = 0; i < 8; i++)
		{
			Rx_28A[i] = RxMessage.Data[i];
		}
	}
	if (RxMessage.StdId == 0x291)
	{
		for (i = 0; i < 8; i++)
		{
			Rx_281[i] = RxMessage.Data[i];
		}
	}
	if (RxMessage.StdId == 0x292)
	{
		for (i = 0; i < 8; i++)
		{
			Rx_282[i] = RxMessage.Data[i];
		}
	}
	if (RxMessage.StdId == 0x181)
	{
		for (i = 0; i < 8; i++)
		{
			Rx_CJ181[i] = RxMessage.Data[i];
		}
	}
	if (RxMessage.StdId == 0x281)
	{
		for (i = 0; i < 8; i++)
		{
			Rx_CJ281[i] = RxMessage.Data[i];
		}
	}
}
//==============================PI_Fliter========================================
u16 PI_Filter(u16 PI_IN)
{
	static u16 value;
	u16 new_value;
	new_value = PI_IN;
	if ((new_value - value > 2200) || (value - new_value > 2200))
	{
		return value;
	}
	value = new_value;
	return new_value;
}
/**
 * @brief 速度滤波函数
 *
 * @param in 输入信号
 * @param len 滤波数组长度
 * @param cyc
 * @param p
 * @param p2
 * @return u16
 */
u16 RC_Speed(u16 in, u8 len, u8 cyc, u16 *p, u32 *p2)
{
	char ii;
	//
#define total *p2
#define kk *(p2 + 1)
#define cyc_m *(p2 + 2)
	//
	cyc_m = cyc_m + 1;
	if (cyc_m >= cyc)
		cyc_m = 0;
	//
	if (cyc < 2 || cyc_m == 0)
	{
		//
		if (kk < len)
			kk = kk + 1;
		//
		for (ii = 0; ii < (len - 1); ii++)
		{
			*(p + len - ii - 1) = *(p + len - ii - 2);
		}
		//
		*p = in;
		//
		total = 0;
		for (ii = 0; ii < kk; ii++)
		{
			total = total + *(p + ii);
		}
	}
	//
	return total / kk;
}
/**
 * @brief  减速斜坡函数
 * @param in 输入信号
 * @param par_dec
 * @param par_delay
 * @param p
 * @return u16
 */
u16 Dec_Option(u16 in, u16 par_dec, u16 par_delay, u16 *p)
{
#define in_m *p
#define cyc_m2 *(p + 1)
#define out_m *(p + 2)
	//
	if (in >= in_m)
	{
		in_m = in;
		out_m = in;
		cyc_m2 = 0;
		return in;
	}
	else
	{
		cyc_m2 = cyc_m2 + 1;
		if (cyc_m2 >= par_delay)
		{
			// if((abs((int)in-(int)in_m))>par_dec)
			if ((in_m - in) > par_dec)
			{
				in_m = in_m - par_dec;
				cyc_m2 = par_delay;
			}
			else
			{
				in_m = in;
				cyc_m2 = 0;
			}
		}
		return in_m;
	}
}

u32 Dec_Option_32(u32 in, u16 par_dec, u16 par_delay, u32 *p)
{
#define in_m *p
#define cyc_m2 *(p + 1)
#define out_m *(p + 2)
	//
	if (in >= in_m)
	{
		in_m = in;
		out_m = in;
		cyc_m2 = 0;
		return in;
	}
	else
	{
		cyc_m2 = cyc_m2 + 1;
		if (cyc_m2 >= par_delay)
		{
			// if((abs((int)in-(int)in_m))>par_dec)
			if ((in_m - in) > par_dec)
			{
				in_m = in_m - par_dec;
				cyc_m2 = par_delay;
			}
			else
			{
				in_m = in;
				cyc_m2 = 0;
			}
		}
		return in_m;
	}
}

u32 Inc_Option(u32 in, u16 par_dec, u16 par_delay, u32 *p)
{
#define in_m *p
#define cyc_m2 *(p + 1)
#define out_m *(p + 2)
	//
	if (in <= in_m)
	{
		in_m = in;
		out_m = in;
		cyc_m2 = 0;
		return in;
	}
	else
	{
		cyc_m2 = cyc_m2 + 1;
		if (cyc_m2 >= par_delay)
		{
			// if((abs((int)in-(int)in_m))>par_dec)
			if ((in - in_m) > par_dec)
			{
				in_m = in_m + par_dec;
				cyc_m2 = par_delay;
			}
			else
			{
				in_m = in;
				cyc_m2 = 0;
			}
		}
		return in_m;
	}
}
